#include <stdint.h>

// 示例：使用位运算进行3点窗口的多数表决滤波
uint32_t binary_bilateral_filter_u32(uint32_t* signal_history, uint32_t new_input) {
    // 更新历史：将新输入移入历史寄存器
    *signal_history = (*signal_history << 1) | new_input;

    // 并行多数表决：对于每个信号，如果3个历史点中有至少2个为1，则输出1。
    uint32_t bit0_mask = *signal_history & 0x55555555; // 提取最早的历史位
    uint32_t bit1_mask = *signal_history & 0xAAAAAAAA; // 提取中间的历史位
    uint32_t bit2_mask = new_input;                    // 当前值作为第3个点

    // 逻辑：输出 = (bit0 & bit1) | (bit0 & bit2) | (bit1 & bit2)
    uint32_t filtered_output = (bit0_mask & bit1_mask) | (bit0_mask & bit2_mask) | (bit1_mask & bit2_mask);
    return filtered_output;
}

/**
 * @brief 针对二值信号的双边滤波（多数表决滤波）
 * @param signal_history 指向一个uint16_t变量，用于存储所有信号的历史状态（每个信号占1位）
 * @param new_input      一个新的uint16_t，其每一位代表对应信号的最新输入值（1或0）
 * @return uint16_t      滤波后的输出，每一位代表对应信号的当前稳定状态
 *
 * @note 此函数假设 signal_history 所指向的变量已经初始化（例如全0）。
 *       每次调用，都会用 new_input 更新历史记录，并返回滤波后的结果。
 *       此示例使用一个3点窗口（2个历史点+1个当前点）进行多数表决。
 */
uint16_t binary_bilateral_filter_u16(uint16_t* signal_history, uint16_t new_input) {
    // 1. 更新历史记录：将新输入移入历史寄存器
    // 将历史状态左移1位，空出最低位，并用新输入填充
    *signal_history = (*signal_history << 1) | new_input;

    // 2. 为每个信号通道进行并行多数表决
    // 我们需要从*signal_history中提取每个信号的3个连续bit位（例如，对于3点窗口）
    // 掩码设计取决于你希望使用的历史窗口长度和信号在uint16_t中的排列方式。
    // 以下是一个针对3点窗口（bit2: 当前输入, bit1: 前一次历史, bit0: 前两次历史）的示例：

    // 提取每个信号的第0位（最早的历史）
    uint16_t bit0_mask = *signal_history & 0x5555; // 二进制掩码：0101 0101 0101 0101
    // 提取每个信号的第1位（中间历史）
    uint16_t bit1_mask = *signal_history & 0xAAAA; // 二进制掩码：1010 1010 1010 1010
    // 最新输入作为第2位（当前值）
    uint16_t bit2_mask = new_input;

    // 3. 计算多数表决：对于3个bit，如果至少有2个为1，则输出1；否则输出0。
    // 逻辑表达式: Output = (b0 & b1) | (b0 & b2) | (b1 & b2)
    // 使用位操作并行地为所有16路信号计算这个表达式
    uint16_t majority_vote = (bit0_mask & bit1_mask) | (bit0_mask & bit2_mask) | (bit1_mask & bit2_mask);

    return majority_vote;
}

uint8_t binary_bilateral_filter_u8(uint8_t* signal_history, uint8_t new_input) {
    // 1. 更新历史记录：将新输入移入历史寄存器
    // 将历史状态左移1位，空出最低位，并用新输入填充
    *signal_history = (*signal_history << 1) | new_input;

    // 2. 为每个信号通道进行并行多数表决
    // 我们需要从*signal_history中提取每个信号的3个连续bit位（例如，对于3点窗口）
    // 掩码设计取决于你希望使用的历史窗口长度和信号在uint16_t中的排列方式。
    // 以下是一个针对3点窗口（bit2: 当前输入, bit1: 前一次历史, bit0: 前两次历史）的示例：

    // 提取每个信号的第0位（最早的历史）
    uint8_t bit0_mask = *signal_history & 0x55; // 二进制掩码：0101 0101
    // 提取每个信号的第1位（中间历史）
    uint8_t bit1_mask = *signal_history & 0xAA; // 二进制掩码：1010 1010
    // 最新输入作为第2位（当前值）
    uint8_t bit2_mask = new_input;

    // 3. 计算多数表决：对于3个bit，如果至少有2个为1，则输出1；否则输出0。
    // 逻辑表达式: Output = (b0 & b1) | (b0 & b2) | (b1 & b2)
    // 使用位操作并行地为所有16路信号计算这个表达式
    uint8_t majority_vote = (bit0_mask & bit1_mask) | (bit0_mask & bit2_mask) | (bit1_mask & bit2_mask);

    return majority_vote;
}